#ifndef ARUCODETECT_H
#define ARUCODETECT_H

#include <opencv2/aruco.hpp>
#include <opencv2/highgui.hpp>
#include <opencv2/aruco/charuco.hpp>
#include <opencv2/highgui.hpp>
#include <opencv2/calib3d/calib3d.hpp>

#include <iostream>
#include <string>

#include <ros/ros.h>
#include <std_msgs/Bool.h>
#include "sensor_msgs/Image.h"
#include "cv_bridge/cv_bridge.h"
#include <tf2_ros/transform_broadcaster.h>
#include <tf2_geometry_msgs/tf2_geometry_msgs.h>
#include <tf2/LinearMath/Quaternion.h>
#include <geometry_msgs/TransformStamped.h>
#include <geometry_msgs/PoseStamped.h>
#include <image_transport/image_transport.h>
#include <yaml-cpp/yaml.h>
// #include "arucoros/MyVectors.h"

class ArucoDetect {
public:
    ArucoDetect(ros::NodeHandle &nh);
    ~ArucoDetect();

private:
    void readCameraParameters(YAML::Node &camera_config, cv::Mat &cameraMatrix, cv::Mat &distCoeffs);
    void readCameraParameters(cv::Mat &cameraMatrix, cv::Mat &distCoeffs);
    void imageCallback(const sensor_msgs::ImageConstPtr &msg);
    void maker_detect_video(cv_bridge::CvImagePtr, cv::Ptr<cv::aruco::Dictionary> &dictionary);
    void maker_pose_estimation(cv_bridge::CvImagePtr, cv::Ptr<cv::aruco::Dictionary> &dictionary, cv::Mat &cameraMatrix, cv::Mat &distCoeffs);

private:
    ros::Subscriber image_sub_;
    ros::Publisher pose_pub_;
    ros::Publisher switch_pub_;
    image_transport::Publisher image_pub_;
    ros::NodeHandle nh_;
    image_transport::ImageTransport it_;
    cv::Mat cameraMatrix, distCoeffs;
    cv::Ptr<cv::aruco::Dictionary> dictionary;
    cv_bridge::CvImagePtr cv_ptr;
    geometry_msgs::TransformStamped tfs;

    // 存下launch文件中的参数
    YAML::Node camera_config;
    double landpad_det_len;
    std::string camera_topic, camera_info;
};

#endif